#include "planes_config.h"

bool PlaneXMLConfigReader::loadFromFile(std::vector<PlaneState_S> &planes,const std::string& filename) {
    pugi::xml_document doc;
    if (!doc.load_file(filename.c_str())) {
        std::cerr << "Error: Failed to load XML file: " << filename << std::endl;
        return false;
    }

    pugi::xml_node planesNode = doc.child("Planes");
    if (!planesNode) {
        std::cerr << "Error: No <Planes> node found in XML" << std::endl;
        return false;
    }

    // 读取飞机数量
    int planesNum = planesNode.child("PlanesNum").text().as_int();
    
    // 清空现有飞机
    planes.clear();
    
    // 预分配空间
    planes.reserve(planesNum);
    
    // 解析每架飞机
    for (pugi::xml_node planeNode : planesNode.children("Plane")) {
        if (planes.size() >= static_cast<size_t>(planesNum)) break;
        
        PlaneConfig config;
        
        config.planeID = planeNode.child("ID").text().as_int();
        
        pugi::xml_node posNode = planeNode.child("Position");
        config.position.longitude = posNode.child("Longitude").text().as_double();
        config.position.latitude = posNode.child("Latitude").text().as_double();
        config.position.altitude = posNode.child("Altitude").text().as_double();
        
        pugi::xml_node velNode = planeNode.child("Velocity");
        config.velocity.TAS = velNode.child("TAS").text().as_double();
        
        pugi::xml_node angleNode = planeNode.child("Angle");
        config.angle.yaw = angleNode.child("Yaw").text().as_double();
        config.angle.roll = angleNode.child("Roll").text().as_double();
        config.angle.pitch = angleNode.child("Pitch").text().as_double();
        
        config.team = planeNode.child("Team").text().as_int();
        config.targetID = planeNode.child("TargetID").text().as_int();
        
        pugi::xml_node radarNode = planeNode.child("Radar");
        if (radarNode) {
            config.radar.state = radarNode.child("State").text().as_int();
        } else {
            config.radar.state = 0; // 默认值
        }
        
        pugi::xml_node controlNode = planeNode.child("Control");
        config.control.roll_ctrl = controlNode.child("RollCtrl").text().as_double();
        config.control.pitch_ctrl = controlNode.child("PitchCtrl").text().as_double();
        config.control.yaw_ctrl = controlNode.child("YawCtrl").text().as_double();
        config.control.throttle = controlNode.child("Throttle").text().as_double();

        // 创建并初始化飞机
        PlaneState_S newPlane;
        applyConfigToPlane(newPlane, config);
        planes.push_back(newPlane);
    }
    
    return true;
}

void PlaneXMLConfigReader::applyConfigToPlane(PlaneState_S& plane, const PlaneConfig& config) {
    // 根据团队和ID设置不同的初始化逻辑
    if (config.team == 1) {
        // 友方飞机初始化
        plane._planeID = config.planeID;
        plane._longitude = config.position.longitude;
        plane._latitude = config.position.latitude;
        plane._altitude = config.position.altitude;
        plane.TAS = config.velocity.TAS;
        plane._yaw = config.angle.yaw;
        plane._roll = config.angle.roll;
        plane._pitch = config.angle.pitch;
        plane._team = config.team;
        plane._targetID = config.targetID;
        plane._raderState = config.radar.state;
        plane._roll_ctrl = config.control.roll_ctrl;
        plane._pitch_ctrl = config.control.pitch_ctrl;
        plane._yaw_ctrl = config.control.yaw_ctrl;
        plane._throttle = config.control.throttle;
        plane.initRadar();
    } 
    else if (config.team == 2) {
        // 敌方飞机初始化
        memset(&plane, 0, sizeof(PlaneState_S)); // 先清零
        plane._planeID = config.planeID;
        plane._longitude = config.position.longitude;
        plane._latitude = config.position.latitude;
        plane._altitude = config.position.altitude;
        plane.TAS = config.velocity.TAS;
        plane._yaw = config.angle.yaw;
        plane._roll = config.angle.roll;
        plane._pitch = config.angle.pitch;
        plane._team = config.team;
        plane._targetID = config.targetID;
        plane._raderState = config.radar.state;
        plane._roll_ctrl = config.control.roll_ctrl;
        plane._pitch_ctrl = config.control.pitch_ctrl;
        plane._yaw_ctrl = config.control.yaw_ctrl;
        plane._throttle = config.control.throttle;
        plane.initRadar();
    }
}